/**
 * @file depth_cluster.h
 * @author tangjf (ryontang@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-10-28
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#ifndef SRC_DEPTH_CLUSTER_H
#define SRC_DEPTH_CLUSTER_H

// include personal lib.
#include <ros/ros.h>
#include "msg_lidar_prep/LidarPoints.h"
#include "msg_lidar_prep/msg_lidar_prep.h"
#include "msg_lidar_shape/DetectedObject.h"
#include "msg_lidar_shape/DetectedObjectArray.h"

#include "n_lidar_obj/util/cloud.hpp"
#include "n_lidar_obj/util/radians.h"
#include "n_lidar_obj/util/richpoint.hpp"
#include "n_lidar_obj/util/useful_typedefs.h"

#include "n_lidar_obj/bounding_box/bounding_box.h"
#include "n_lidar_obj/bounding_box/obj_visualization.h"
#include "n_lidar_obj/pcl_process/processPointClouds.h"
#include "n_lidar_obj/render/render.h"
#include "n_lidar_obj/depth_cluster/curvevoxel_cluster.h"

// include system lib.
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>
#include <condition_variable>
#include <mutex>
#include <opencv2/opencv.hpp>
#include <Eigen/StdVector>

// Include the test lab for point 3d
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include "geometry_msgs/Point32.h"
#include "sensor_msgs/ChannelFloat32.h"
#include "sensor_msgs/PointCloud.h"
#include "std_msgs/Header.h"

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#if PCL_FOUND
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#endif // PCL_FOUND

#include <algorithm>
#include <cstdint>
#include <list>
#include <memory>
#include <stdexcept>
#include <vector>
#include <limits.h>

/**
 * @brief 引用上面声明的类与结构体，构成一个组合类，功能是实现深度分割聚类
 *
 */
class Depth_cluster // 可以使用基类的共有成员
{
public:
  // 一列点
  using PointColumn = std::vector<PointContainer>;
  // vector < vector<list> > 点的Matrix
  using PointMatrix = std::vector<PointColumn>;

  Depth_cluster() = default;
  //{

  //obj_visualization = new ObjVisualization("obj_car");
  //_obj_cluster(new std::unordered_map<uint16_t, depth_clustering::Cloud>);

  // _data = PointMatrix(_params.cols(), PointColumn(_params.rows()));
  // _data = PointMatrix(1800, PointColumn(32));

  //_depth_image =
  //        cv::Mat::zeros(_params.rows(), _params.cols(),
  //        cv::DataType<float>::type);

  //_data = PointMatrix(1801, PointColumn(32));
  //_data = PointMatrix(800+1, PointColumn(32));
  //}
  ~Depth_cluster()
  {
    //delete obj_visualization;
  }
  // inline const PointMatrix& matrix() const { return _data; }
  cv::Mat &depth_image();
  const cv::Mat &depth_image() const;

  void Init();

  void SpanParams(const Radians &start_angle, const Radians &end_angle, int num_beams); // lidar params to project

  size_t ColFromAngle(const Radians &angle);
  size_t FindClosest(const std::vector<Radians> &vec, const Radians &val);

  void CloudProjection(const msg_lidar_prep::msg_lidar_prep::ConstPtr &points); // pointcloud trans to depth_image

  cv::Mat CreateAngleImage(const cv::Mat &depth_image); // Create a Angle Image object

  int MYZeroOutGroundBFS(const cv::Mat &depth_image, const cv::Mat &angle_image,
                         cv::Mat &no_ground_image, cv::Mat &label_image,
                         const float &start_ground_threshold, int kernel_size,
                         PointMatrix &datalist, const Cloud::Ptr cloud_set); // segmentation，call ground_BFS()

  void ground_BFS(const PixelCoord &start, uint16_t label, cv::Mat &label_image,
                  const cv::Mat &depth_image, const cv::Mat &angle_image,
                  const float &ground_remove_angle); // BFS of ground points

  int Cluster(const cv::Mat &no_ground_depth_image, cv::Mat &label_image,
              PointMatrix &datalist, Cloud::Ptr cloud_set,
              const float &angle_tollerance, int min_cluster_size,
              int max_cluster_size); // depth cluster, call One_Label_Bfs()

  void One_Label_Bfs(const PixelCoord &start, uint16_t label,
                     cv::Mat &label_image, const cv::Mat &no_grond_depth_image,
                     const float &angle_tollerance); // BFS of cluster search

  std::vector<PointCoord> Neighbourhood(int16_t coor_row, int16_t coor_col);

  cv::Mat ApplySavitskyGolaySmoothing(const cv::Mat &image, int window_size);

  cv::Mat ZeroOutGround(const cv::Mat &image, const cv::Mat &angle_image,
                        const Radians &threshold) const;
  cv::Mat GetUniformKernel(int window_size, int type) const;

  cv::Mat GetSavitskyGolayKernel(int window_size) const;

  cv::Mat RepairDepth(const cv::Mat &no_ground_image, int step,
                      float depth_threshold);

  void RvizVisual(std::unordered_map<uint16_t, Cloud> &visual_clusters,
                  ros::Publisher puber);

  void RvizVisual(Cloud::Ptr visual_clusters, ros::Publisher puber);

  void RvizVisual(const msg_lidar_prep::msg_lidar_prep::ConstPtr &points,
                  ros::Publisher puber); // rviz data_type of msg_lidar_prep::msg_lidar_prep::ConstPtr

  void Rviz(); // 封装发布markers的实现

  sensor_msgs::Image DepthImage_to_ImageMsg(cv::Mat depth_image, int threshold_distance = 60); // 深度图转图像

  sensor_msgs::Image DepthImageAndAngleImage_to_ImageMsg(cv::Mat depth_image, cv::Mat repaired_depth_image,
                                                         cv::Mat angle_image,
                                                         cv::Mat smoothed_angle_image,
                                                         cv::Mat no_ground_depth_image); // 可视化好几个图像

  void ViewerPcl(ProcessPointClouds<pcl::PointXYZI> *point_processor,
                 pcl::visualization::PCLVisualizer::Ptr &viewer); // pcl visualise call CityBlock()

  void CityBlock(const std::unordered_map<uint16_t, Cloud> &visual_clusters,
                 Cloud::Ptr ground_cloud,
                 pcl::visualization::PCLVisualizer::Ptr &viewer,
                 ProcessPointClouds<pcl::PointXYZI> *point_processor); // pcl visualise call RenderPointCloud() and BoundingBox(pcl_cluster)

  pcl::PointCloud<pcl::PointXYZI>::Ptr CloudToPcl(const Cloud &PointCloud);
  pcl::PointCloud<pcl::PointXYZI>::Ptr CloudToPcl(const Cloud::Ptr PointCloud);

  void RenderPointCloud(pcl::visualization::PCLVisualizer::Ptr &viewer,
                        const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud,
                        std::string name, Color color);

  // Cloud _cloud;
  // static constexpr int16_t NEIGH_SIZE = 2 * 1 + 2 * 1;
  // std::array<PixelCoord, NEIGH_SIZE> _Neighborhood;
  // std::vector<PointCoord> _Neighborhood;
  cv::Mat _depth_image;
  cv::Mat _repaired_depth_image;
  cv::Mat _angle_image;
  cv::Mat _smoothed_angle_image;
  cv::Mat _no_ground_image;
  cv::Mat _no_ground_label_image;
  // cv::Mat _angle_image;
  cv::Mat _xy_image;
  cv::Mat _z_image;
  cv::Mat _angle_arith_image;
  cv::Mat _angle_vert_image;

  // 梯度图
  cv::Mat _tensor_image;

  cv::Mat _label_image;
  // just stores addresses of the points. Does not own them.
  // PointMatrix _data; // vector < vector<list> >
  size_t bin_cols_copy;

  int _count_ground;
  float _hres = 0.45;
  float _ground_remove_angle = 45.0;
  float _ground_tollerance = 5.0;
  float _angle_tollerance = 8.0;
  int _min_cluster_size = 100;
  // int _max_cluster_size = 5000;
  int _max_cluster_size = 25000;
  int _window_size = 5;
  std::vector<Radians> _col_angles;
  Radians _start_angle = 0_deg;
  Radians _end_angle = 0_deg;
  Radians _step = 0_deg;
  Radians _span = 0_deg;
  int _num_beams = 0;
  // Cloud::Ptr _cloud  = nullptr;
  //Cloud::Ptr _cloud;
  PointMatrix _data;
  // std::unordered_map<uint16_t, Cloud> _clusters;
  // std::unordered_map<uint16_t, Cloud*> _clusters;
  Cloud::Ptr _ground_cloud; // 类的成员生存期与所属对象的生存期保持一致
  Cloud::Ptr _except_ground_cloud;
  std::unordered_map<uint16_t, Cloud> _obj_cluster;
  msg_lidar_prep::msg_lidar_prep _shape_pointcloud;

  // vector<Cloud> _obj_cluster;
  int _cluster_count = 0; // 聚类的数目
  int _pub_flag = 0;
  int _rviz_pub = 1;
  // Cloud::Ptr _cloud;
  // Ros
  ros::NodeHandle n;
  ros::Subscriber sub_lidar_ring_prep; // ROS Subscriber msg_lidar_prep then
                                       // dealt with ring extract
  ros::Publisher _pub_depth_cluster;
  ros::Publisher _pub_ground_cluster;
  ros::Publisher _pub_cloud;
  ros::Publisher _pub_marker_array;
  ros::Publisher _image_pub;
  int _cluster_type;
  // sensor_msgs::PointCloud _msg_lidar_prep_rviz;

  // visualize.h
  ObjVisualization *obj_visualization;
  msg_lidar_shape::DetectedObjectArray _obj_visualization_array;

  // mutex
  std::mutex _mu;
  std::condition_variable _cond;

private:
  visualization_msgs::Marker shape_estimate(const Cloud &cluster, int setid,
                                            double centroid_x,
                                            double centroid_y,
                                            double centroid_z, double width,
                                            double length, double height);

  visualization_msgs::Marker CreateWireframeMarker(
      const float &center_x, const float &center_y, const float &center_z,
      const float &size_x, const float &size_y, const float &size_z); // Create a Wireframe Marker object
};

#endif // SRC_DEPTH_CLUSTER_H
